import os
from launch import LaunchDescription
from launch_ros.actions import Node, ComposableNodeContainer
from launch_ros.descriptions import ComposableNode
from ament_index_python.packages import get_package_share_directory

def generate_launch_description():
    # 获取apriltag_ros包的路径
    apriltag_ros_share_dir = get_package_share_directory('apriltag_ros')
    
    node_params = os.path.join(
          get_package_share_directory('ros2_video'), 'config', 'node_params.yaml')
     
    camera_info_url = 'package://ros2_video/config/camera_info.yaml'
    
    ros2_video_node = Node(
        package='ros2_video',
        executable='ros2_video_node',
        parameters=[node_params, {'camera_info_url': camera_info_url}],
        output='screen',
        emulate_tty=True,
        remappings=[
        ('/image_raw', '/camera/image_raw'),
        ('/camera_info', '/camera/camera_info')
    ])

    # 创建ComposableNode列表
    composable_nodes = [
        ComposableNode(
            package='image_proc',
            plugin='image_proc::RectifyNode',
            name='rectify',
            namespace='camera',
            remappings=[
                ('/camera/image', '/camera/image_raw'),
                ('/camera/camera_info', '/camera/camera_info')
            ]
        ),
        ComposableNode(
            package='apriltag_ros',
            plugin='AprilTagNode',
            name='apriltag',
            namespace='apriltag',
            parameters=[os.path.join(apriltag_ros_share_dir, 'cfg', 'tags_36h11.yaml')],
            remappings=[
                ('/apriltag/image_rect', '/camera/image_raw'),
                ('/apriltag/camera_info', '/camera/camera_info')
            ]
        )
    ]

    # 创建ComposableNodeContainer
    container = ComposableNodeContainer(
        name='apriltag_container',
        namespace='',
        package='rclcpp_components',
        executable='component_container',
        composable_node_descriptions=composable_nodes
    )

    return LaunchDescription([
        ros2_video_node, 
        container
        ])